//
// Created by k on 25-6-26.
//

#ifndef UNITREE_B2_H
#define UNITREE_B2_H

#include <string>
#include <unitree/robot/b2/sport/sport_client.hpp>
#include <unitree/robot/go2/sport/sport_client.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/SportModeState_.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
#include <unitree/common/time/time_tool.hpp>
// #include <iostream>
// #include <stdio.h>
// #include <stdint.h>
// #include <math.h>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/common/time/time_tool.hpp>
#include <unitree/common/thread/thread.hpp>
#include "chassis.h"
enum motion_mode
{
    /*---Basic motion---*/
    normal_stand,
    balance_stand,
    velocity_move,
    stand_down,
    stand_up,
    damp,
    recovery_stand,
    /*---Special motion ---*/
    sit,
    rise_sit,
    stop_move = 99
  };

class UnitreeB2 {
public:
    std::string network_interface_ ;
    explicit UnitreeB2(std::string network_interface) ;
    void set_network_interface(const std::string &network_interface){this->network_interface_ = network_interface; }

    // ~UnitreeB2();
    void HighStateHandler(const void *message);
    void LowStateMessageHandler(const void *message);
    void RobotControl() ;

    unitree::robot::b2::SportClient sport_client_;
    unitree_go::msg::dds_::SportModeState_ sport_mode_state_;
    unitree::robot::go2::RobotStateClient robot_state_client_;
    unitree_go::msg::dds_::LowState_ low_state_{};  // default init

    unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> sport_mode_state_suber_;
    unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> low_state_suber_;

    double px0, py0, yaw0; // 初始状态的位置和偏航
    double current_time_ = 0; // 运行时间
    int flag = 0; // 特殊动作执行标志
    float dt_ = 0.005; // 控制步长0.001~0.01
    ChassisData data_;
};
#endif //UNITREE_B2_H
